#include "total_inventory.h"

//-----——————————————————————-----—————————————————— 常用算法————————----————————————————----——————————————---————————

//————————————————————————————————PID各类控制方法——————————————————————————————————————————————————————
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     进阶版PID控制的结构体初始化   TODO
// 参数说明     SteeringParams * Params 引入结构体参数
// 返回参数     void
// 使用示例
// 备注信息
//---------------------------------------------------------------------------------------------------------------
// 初始化函数
void SteeringParams_Init(SteeringParams* params) {
    if (params == NULL) {
        return; // 防止空指针
    }
    params->kp = 0.0f;                   // 比例系数初始为0
    params->kp2 = 0.0f;                  // 非线性比例系数初始为0
    params->kd = 0.0f;                   // 微分系数初始为0
    params->gkd = 0.0f;                  // 陀螺仪微分系数初始为0

    params->ki_er = 0.01f;               // 积分增益，置于范围多大内设置条件，需要在执行函数里面修改
    params->ki_output = 0.0f;            // 积分项输出初始为0
    params->DT = 0.01f;                  // 运算步长，基于10ms中断周期
    params->last_image_deviation = 0.0f; // 上一次误差初始为0

    params->pwm_max = 1000;              // 最大值，假设为1000（可根据硬件调整）这里如果不适用直接为0即可
    params->pwm_min = -1000;             // 最小值，假设为-1000（可根据硬件调整）这里如果不适用直接为0即可
    params->left_motor_pwm = 0;          // 左电机输出初始为0
    params->right_motor_pwm = 0;         // 右电机输出初始为0
}



//-------------------------------------------------------------------------------------------------------------------
// 函数简介     进阶版PID控制(用于位置控制)    TODO
// 参数说明     SteeringParams * Params 引入结构体参数
// 参数说明     image_deviation         误差
// 参数说明     yaw_rate                陀螺仪数值(根据情况选定坐标系)，做为阻尼作用注意需要取反
// 返回参数     void
// 使用示例
// 备注信息     需要结合陀螺仪使用，优先调整KP，然后根据情况调整gkd,然后调整KP2来加快响应速度，最后调整KI和KD，用来i小凹处稳态误差和加快反应速度
//---------------------------------------------------------------------------------------------------------------

// 转向控制函数 进阶PID，不至于转向控制
void steering_control(SteeringParams * params,float image_deviation, float yaw_rate) {
    static float Symbol_ki = 0;     // 用来标志是否开启积分

    if(image_deviation < 30 || image_deviation > -30)  // 限制误差的范围内进行积分，保证系统消除稳态误差
    {
        Symbol_ki = 1;
        params -> ki_output += params -> ki_er * image_deviation * params -> DT;  // 积分累加消除稳态误差

    }else
    {
        Symbol_ki = 0;  // 标志位清零
        params -> ki_output = 0;  // 积分清零
    }

    // 计算转向值
    float steer_value = 0.0f;  //  进阶PID的输出值，这里面暂时用于左右电机的输出同时也可以用于其他输出
    float deviation_abs = (image_deviation >= 0) ? image_deviation : -image_deviation;   // 取绝对值用于计算

    // 转向值 = (图像偏差) * KP + (图像偏差) * abs(图像偏差) * KP2 + (当前图像偏差 - 上一次图像偏差) * KD + 偏航角速度 * GKD
    steer_value = (image_deviation * params->kp) +
                  (image_deviation * deviation_abs * params->kp2) +
                  ((image_deviation - params -> last_image_deviation) * params->kd) +
                  (yaw_rate * params->gkd);

    if(Symbol_ki == 1)
    {
        steer_value = steer_value + params -> ki_output;
    }

    // 更新上一次图像偏差
    params -> last_image_deviation = image_deviation;

    //————————————————————————————————————如果用于其他控制，那么直接忽略就行，即注释掉这部分，使用前面的steer_value——————————————
    // 限制幅度防止超限
    params->left_motor_pwm = (params->left_motor_pwm > params->pwm_max) ? params->pwm_max : params->left_motor_pwm;
    params->left_motor_pwm = (params->left_motor_pwm < params->pwm_min) ? params->pwm_min : params->left_motor_pwm;

    params->right_motor_pwm = (params->right_motor_pwm > params->pwm_max) ? params->pwm_max : params->right_motor_pwm;
    params->right_motor_pwm = (params->right_motor_pwm < params->pwm_min) ? params->pwm_min : params->right_motor_pwm;

    // 根据转向值调整左右电机PWM，
    // steer_value决定转向差速
    params->left_motor_pwm = (int)steer_value;
    params->right_motor_pwm= -(int)steer_value;
    //————————————————————————————————————如果用于其他控制，那么直接忽略就行，即注释掉这部分，使用前面的steer_value——————————————

}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     普通位置式PID控制的初始化  TODO
// 参数说明     PIDController *pid      引入结构体参数
// 返回参数     void
// 使用示例
// 备注信息
//-------------------------------------------------------------------------------------------------------------------

// 初始化PID控制器
void PID_Init(PIDController *pid) {
    pid->kp = 0.0f;
    pid->ki = 0.0f;
    pid->kd = 0.0f;
    pid->setpoint = 0.0f;
//————积分限幅———
    pid->integral_max = 0.0f;
    pid->integral_min = 0.0f;
//————积分限幅———

//————总输出限幅——————
    pid->output_max = 0.0f;
    pid->output_min = 0.0f;
//————总输出限幅——————

    pid->integral = 0.0f;
    pid->last_error = 0.0f;     // 接收上一次的误差参数以及存储积分参数
}



//-------------------------------------------------------------------------------------------------------------------
// 函数简介     普通位置式PID      TODO
// 参数说明     PIDController *pid      引入结构体参数
// 参数说明     measured_value          传入的传感器反馈值
// 参数说明     dt                      运算步长，单片机里面可以以中断周期为主
// 返回参数     float                   PID输出值
// 使用示例
// 备注信息
//-------------------------------------------------------------------------------------------------------------------


// 位置式PID计算函数
float PID_Compute(PIDController *pid, float measured_value, float dt) {
    // 计算误差
    float error = pid->setpoint - measured_value;

    // 比例项
    float p_term = pid->kp * error;

    // 积分项（含限幅）
    pid->integral += pid->ki * error * dt;
    if (pid->integral > pid->integral_max) {
        pid->integral = pid->integral_max;
    } else if (pid->integral < pid->integral_min) {
        pid->integral = pid->integral_min;
    }
    float i_term = pid->integral;

    // 微分项
    float d_term = pid->kd * (error - pid->last_error) / dt;
    pid->last_error = error;

    // 计算输出
    float output = p_term + i_term + d_term;

    // 输出限幅
    if (output > pid->output_max) {
        output = pid->output_max;
    } else if (output < pid->output_min) {
        output = pid->output_min;
    }

    return output;
}


//-------------------------------------------------------------------------------------------------------------------
// 函数简介     增量式PID的初始化  TODO
// 参数说明     IncrementalPID *pid 引入结构体参数
// 返回参数     void
// 使用示例
// 备注信息
//-------------------------------------------------------------------------------------------------------------------

// 初始化增量式PID控制器
void IncrementalPID_Init(IncrementalPID *pid) {
    if (pid == NULL) {
        return; // 防止空指针
    }
    pid->kp = 0.0f;                   // 比例增益初始为0
    pid->ki = 0.0f;                   // 积分增益初始为0
    pid->kd = 0.0f;                   // 微分增益初始为0
    pid->setpoint = 0.0f;             // 目标值初始为0
    pid-> integral_max = 100.0f;      // 积分限幅最大值初始为100
    pid->integral_min = -100.0f;      // 积分限幅最小值初始为-100
    pid->output_max = 1000.0f;        // 输出限幅最大值初始为1000
    pid->output_min = -1000.0f;       // 输出限幅最小值初始为-1000

    pid->integral = 0.0f;             // 积分项初始为0
    pid->last_error = 0.0f;           // 上一次误差初始为0
    pid->prev_error = 0.0f;           // 上上一次误差初始为0
    pid->last_output = 0.0f;          // 上一次输出初始为0
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     增量式PID         TODO
// 参数说明     IncrementalPID *pid     引入结构体参数
// 参数说明     measured_value          传入的传感器反馈值
// 参数说明     dt                      运算步长，单片机里面可以以中断周期为主
// 返回参数     float                   PID输出值
// 使用示例
// 备注信息
//-------------------------------------------------------------------------------------------------------------------


// 增量式PID计算函数
float IncrementalPID_Compute(IncrementalPID *pid, float measured_value) {
    // 计算当前误差
    float error = pid->setpoint - measured_value;

    // 增量式PID算法
    float p_term = pid->kp * (error - pid->last_error);                    // 比例增量
    float i_term = pid->ki * error ;                                   // 积分增量
    float d_term = pid->kd * (error - 2.0f * pid->last_error + pid->prev_error) ; // 微分增量

    // 更新积分项并进行限幅
    pid->integral = i_term;
    if (pid->integral > pid->integral_max) {
        pid->integral = pid->integral_max;
    } else if (pid->integral < pid->integral_min) {
        pid->integral = pid->integral_min;
    }

    // 计算输出增量
    float output_increment = p_term + pid->integral + d_term;

    // 计算总输出并进行限幅
    float output = pid->last_output + output_increment;
    if (output > pid->output_max) {
        output = pid->output_max;
    } else if (output < pid->output_min) {
        output = pid->output_min;
    }

    // 更新状态
    pid->prev_error = pid->last_error;
    pid->last_error = error;
    pid->last_output = output;

    return output;
}


//————————————————————————————————滤波方法——————————————————————————————————————————————————————
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     卡尔曼滤波参数初始化  TODO
// 参数说明     q1          误差过程的噪声
// 参数说明     q2          误差变化率过程噪声
// 参数说明     r           测量误差噪声
// 参数说明     dt          运算步长，单片机里面可以以中断周期为主
// 返回参数     void
// 使用示例
// 备注信息     误差变化模型，只传入误差值，通过模型预测变化率再返回来校正误差值，通过设置噪声的协方差矩阵来调节滤波效果
//-------------------------------------------------------------------------------------------------------------------

void init_kalman_filter(KalmanFilter* filter, float q1, float q2, float r, float dt) {
    filter->x_est = 0.0f;
    filter->v_est = 0.0f;
    filter->p_est_00 = 1.0f;
    filter->p_est_01 = 0.0f;
    filter->p_est_10 = 0.0f;
    filter->p_est_11 = 1.0f;      // 初始过程协方差数值不用改变
    filter->q1 = q1;              // 误差的噪声     过程噪声 值越大越不信任过程值，更加信任测量值
    filter->q2 = q2;              // 变化率的噪声   过程噪声  值越大越不信任过程值，更加信任测量值，所以该值越小响应速度越快
    filter->r = r;               // 测量噪声协方差   值越大越不信任测量值，数据线变化越激烈，但容易导致输出抖动。值越小则相反，更加平滑
    filter->dt = dt;
    filter->prediction_horizon = 0.003; // 卡尔曼滤波预测时间，时间越短，响应越灵敏，时间越长，越依赖估计的变化率准确性，否则会产生偏差
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     卡尔曼滤波函数    TODO
// 参数说明     KalmanFilter* filter    传入结构体参数
// 参数说明     measurement             传入当前误差
// 返回参数     float                   滤波后的值
// 使用示例
// 备注信息     误差变化模型，只传入误差值，通过模型预测变化率再返回来校正误差值，通过设置噪声的协方差矩阵来调节滤波效果
// 备注信息     代码无法服用多种情况，仅适用于单个值的变化
//-------------------------------------------------------------------------------------------------------------------
// 卡尔曼滤波函数，模型不通用，不同模型需要不同的函数实现
float kalman_filter_predict(KalmanFilter* filter, float measurement) {
    // 预测步骤
    float x_pred = filter->x_est + filter->v_est * filter->dt;
    float p_pred_00 = filter->p_est_00 + filter->dt * (filter->p_est_10 + filter->p_est_01 + filter->dt * filter->p_est_11) + filter->q1;
    float p_pred_01 = filter->p_est_01 + filter->dt * filter->p_est_11;
    float p_pred_10 = filter->p_est_10 + filter->dt * filter->p_est_11;
    float p_pred_11 = filter->p_est_11 + filter->q2;

    // 更新步骤
    float k0 = p_pred_00 / (p_pred_00 + filter->r);  // 卡尔曼增益（误差）
    float k1 = p_pred_10 / (p_pred_00 + filter->r);  // 卡尔曼增益（变化率）
    filter->x_est = x_pred + k0 * (measurement - x_pred);
    filter->v_est = filter->v_est + k1 * (measurement - x_pred);
    filter->p_est_00 = (1.0f - k0) * p_pred_00;
    filter->p_est_01 = p_pred_01 - k1 * p_pred_00;
    filter->p_est_10 = p_pred_10 - k0 * p_pred_10;
    filter->p_est_11 = p_pred_11 - k1 * p_pred_10;

    // 预测未来误差
    float predicted_error = filter->x_est + filter->v_est * filter->prediction_horizon;

    return predicted_error;
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     一阶低通滤波参数初始化   TODO
// 参数说明     LowPassFilter *filter  引入结构体参数
// 参数说明     cutoff_frequency       截止频率
// 参数说明     sample_time            采样时间
// 返回参数     void
// 使用示例
// 备注信息     截止频率一般要小于采样频率的一半，我这里采样频率为100hz,那么截止频率不能高于50hz，越接近50hz，滤波效果越差，相位延迟越低
//-------------------------------------------------------------------------------------------------------------------

#define PI               3.14159265358979f
// 初始化滤波器
void LowPassFilter_Init(LowPassFilter *filter, float cutoff_frequency, float sample_time) {
    // 计算滤波系数 alpha
    float tau = 1.0f / (2.0f * PI * cutoff_frequency); // 时间常数 tau = 1 / (2*pi*fc)
    filter->alpha = exp(-sample_time / tau);             // alpha = e^(-T/tau)
    filter->previous_output = 0.0f;                     // 初始输出设为0
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     一阶低通滤波      TODO
// 参数说明     LowPassFilter *filter  引入结构体参数
// 参数说明     input                  需要滤波的值
// 返回参数     float                  返回滤波后的值
// 使用示例
// 备注信息     截止频率一般要小于采样频率的一半，我这里采样频率为100hz,那么截止频率不能高于50hz，越接近50hz，滤波效果越差，相位延迟越低
//-------------------------------------------------------------------------------------------------------------------

// 执行滤波操作
float LowPassFilter_Update(LowPassFilter *filter, float input) {
    // 根据公式计算当前滤波输出
    float output = filter->alpha * filter->previous_output + (1.0f - filter->alpha) * input;
    // 更新上一次的输出值
    filter->previous_output = output;
    return output;
}


//-------------------------------------------------------------------------------------------------------------------
// 函数简介     查表法初始化(用于计算tanh函数)  TODO
// 参数说明     TanhTable *table       引入结构体参数
// 参数说明     min                    表的范围里的最小值
// 返回参数     max                    表的范围里的最大值
// 使用示例
// 备注信息     这里范围越小对于tanh来说越平滑，而范围越小越容易达到饱和
//            这种非线性缩放的方法，在小误差的时候变化比较缓慢，大误差的时候变化比较抖，与指数平滑相反
//-------------------------------------------------------------------------------------------------------------------

// 初始化查表
void init_tanh_table(TanhTable *table, float min, float max, int size)
{
    if (size <= 0 || size > 1000) {
        return;
    }
    // 设置结构体字段
    table->x_min = min;
    table->x_max = max;
    table->table_size = size;

    // 填充查表
    float step = (max - min) / (size - 1);
    for (int i = 0; i < size; i++) {
        float x = min + i * step;
        table->tanh_table[i] = tanh(x);  // 使用标准库函数预先计算
    }

}



//-------------------------------------------------------------------------------------------------------------------
// 函数简介     查表法计算tanh函数用于非线性缩放     TODO
// 参数说明     TanhTable *table       引入结构体参数
// 参数说明     x                      传入的数值，但是还需要引入敏感系数，这里暂未作为传递参数
// 返回参数     max                    表的范围里的最大值
// 使用示例
// 备注信息     这里范围越小对于tanh来说越平滑，而范围越小越容易达到饱和
//-------------------------------------------------------------------------------------------------------------------


// 使用查表法计算 tanh
float tanh_lookup(const TanhTable *table, float x) {
    if (table->table_size == 0) {
        printf("Error: Tanh table is not initialized.\n");
        return 0.0;
    }

    // 超出范围处理
    if (x <= table->x_min) x = table->x_min;
    if (x >= table->x_max) x = table->x_max;

    // 计算步长和索引
    float step = (table->x_max - table->x_min) / (table->table_size - 1);
    int index = (int)((x - table->x_min) / step);

    // 线性插值
    float x_low = table->x_min + index * step;
    float x_high = table->x_min + (index + 1) * step;
    float y_low = table->tanh_table[index];
    float y_high = table->tanh_table[index + 1];

    return y_low + (y_high - y_low) * (x - x_low) / (x_high - x_low);
}




//-------------------------------------------------------------------------------------------------------------------
// 函数简介     int类型拆成四个字节，设置参数的时候需要这么操作   TODO
// 参数说明     value           输入参数，32位的int类型
// 参数说明     *data           储存地址，即拆出来的字节的位置，可以放入数组
// 返回参数     void
// 使用示例
// 备注信息     两个无符号的八位整型转化为一个有符号的16位整型
//-------------------------------------------------------------------------------------------------------------------

void plit_int_to_bytes(int32_t value, uint8_t *data) {
    // 提取每个字节并存储到指定位置
    data[5] = (value >> 24) & 0xFF;  // 高字节
    data[4] = (value >> 16) & 0xFF;  // 次高字节
    data[3] = (value >> 8) & 0xFF;   // 次低字节
    data[2] = value & 0xFF;          // 低字节
}



//-------------------------------------------------------------------------------------------------------------------
// 函数简介     高低字节融合整形     TODO
// 参数说明     high_byte      输入参数，高字节
// 参数说明     low_byte       输入参数，低字节
// 返回参数     int16_t        16位整形，不能是32位，否则数据不正确
// 使用示例
// 备注信息     两个无符号的八位整型转化为一个有符号的16位整型
//-------------------------------------------------------------------------------------------------------------------

int16_t Merge_uint8(uint8_t high_byte,uint8_t low_byte)
{
    return ((int16_t)high_byte << 8) | low_byte;
}



//-------------------------------------------------------------------------------------------------------------------
// 函数简介     查表法初始化(用于计算指数平滑函数)   TODO
// 参数说明     ExpSmoothingTable *est       引入结构体参数
// 参数说明     sigma                        缩放因子  //sigma越小 增长越快，调整更敏感   sigma越大 变化越平缓，调整更温和
// 返回参数     e_max                        误差的最大值，用来调整步长
// 返回参数     etable_size_max              用户规定的表最大数量，这里规定1000，为了节省内存你可以减小
// 使用示例     init_exp_smoothing_table(&Exp, 4000.2, 32768, 1000);
// 备注信息     指数平滑对于小误差比较严格，对于大误差变化比较缓和，而tanh对于中间的误差值比较灵敏
//
//-------------------------------------------------------------------------------------------------------------------

// 初始化查表结构体（静态存储）
void init_exp_smoothing_table(ExpSmoothingTable *est, float sigma, float e_max, int table_size) {
    // 确保表大小不超过定义的最大值
    if (table_size > 1000) {
        table_size = 1000;
    }

    // 初始化结构体成员
    est->table_size = table_size;
    est->sigma = sigma;
    est->e_max = e_max;
    est->step = (e_max / sigma) / (table_size - 1);

    // 预计算表值：table[i] = 1 - exp(-x)，其中 x = i * step
    for (int i = 0; i < table_size; i++) {
        float x = i * est->step;
        est->table[i] = 1.0 - exp(-x);
    }
}


//-------------------------------------------------------------------------------------------------------------------
// 函数简介     查表法计算指数平滑函数用于非线性缩放     TODO
// 参数说明     ExpSmoothingTable *est       引入结构体参数
// 参数说明     e                            误差值
// 返回参数     kp                           系数增益，调整增益大小，因为误差范围数据太大
// 使用示例
// 备注信息     这里主要查看缩放因子的值，如果误差值大，则需要将缩放因子也调大，防止过快到达1(极限)缩放的范围[0，1]
//-------------------------------------------------------------------------------------------------------------------


// 查表函数：根据误差 |e| 计算指数平滑值 f(|e|) = 1 - exp(-|e|/sigma)
float exp_smoothing_lookup(const ExpSmoothingTable *est, float e,float kp) {
    float Location = 0.0f;  // 校正方向，用来保证返回值的方向
    if(e > 0.0f)
    {
        Location = 1.0f;
    }
    if(e < 0.0f)
    {
        Location = -1.0f;
    }
    // 取误差绝对值
    e = fabs(e);

    // 限制误差范围
    if (e > est->e_max) {
        e = est->e_max;
    }

    // 计算对应的 x = |e| / sigma
    float x = e / est->sigma;

    // 映射到表索引
    float index = x / est->step;
    int idx_lower = (int)index;
    int idx_upper = idx_lower + 1;

    // 边界检查
    if (idx_lower < 0) {
        idx_lower = 0;
        idx_upper = 1;
    }
    if (idx_upper >= est->table_size) {
        idx_upper = est->table_size - 1;
        idx_lower = idx_upper - 1;
        return est->table[idx_upper]; // 直接返回最大值
    }

    // 线性插值
    float fraction = index - idx_lower;
    float value_lower = est->table[idx_lower];
    float value_upper = est->table[idx_upper];
    return Location *(value_lower + fraction * (value_upper - value_lower));
}


//-------------------------------------------------------------------------------------------------------------------
// 函数简介     转速的RPM单位转化为弧度制   TODO
// 参数说明     无
// 返回参数     float                  rad/s的单位
// 使用示例
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
float rpm_to_rads(float rpm) {
    return (2 * M_PI * rpm) / 60;
}


//-------------------------------------------------------------------------------------------------------------------
// 函数简介     编码器过零检测结构体初始化  TODO
// 参数说明     Handle_encoder_zero *state  引入结构体参数
// 返回参数     void
// 使用示例
// 备注信息     state->total_angle 作为PID的检测值，用于控制电机旋转绝对角度
//-------------------------------------------------------------------------------------------------------------------
// 初始化
void Encoder_zero_init(Handle_encoder_zero *state) {
    state->first_reading = true;   // 初始上电角度检测标志位
    state->Actual_angle = 0;       // 当前角度(编码器值)
    state->last_angle = 0;         // 储存上一次的角度(编码器值)
    state->raw_delta = 0;          // 对角度误差进行取模运算，取最小路径，接收过零后的差值
    state->total_angle = 0;        // 检测当前电机旋转的绝对角度，可以是任何圈数，且该值用于差值计算，作为检测的值
    state->over_zero_count = 0;    // 过零的圈数，叠加角度
    state->Actual_speed = 0;       // 当前的角速度，用于过零检测
}


//-------------------------------------------------------------------------------------------------------------------
// 函数简介     绝对编码器值处理,过零处理    TODO
// 参数说明     Handle_encoder_zero *state   引入相关结构体参数
// 参数说明     high_byte_angle              接收的角度编码器的高字节  Can_rxData[4]
// 参数说明     low_byt_angle                接收的角度编码器的低字节  Can_rxData[5]
// 参数说明     high_byte_speed              接收的角速度的高字节      Can_rxData[0]
// 参数说明     low_byt_speed                接收的角度编码器的低字节   Can_rxData[1]   这里对应电流反馈的数据 返回的标识符 0x50 + ID
// 内部参数说明  MAX_encoder                  电机旋转一圈可以发射的最大编码器值
// 内部参数说明  Half_MAX_encoder             电机旋转一圈可以发射的编码器值的一半
// 返回参数     void
// 使用示例
// 备注信息     直接控制电机转动绝对角度，将这个角度可以记录下来，然后控制的时候就可以转到绝对的角度，需要配合速度方向来进行过零检测
// 备注信息     放置在CAN接收中断里面调用，代码初始化的时候需要发送0电流来反馈数据，然后就可以得到上电的初始位置
// 备注信息     由于转速的分辨率为0.1，但是我没有除以10，所以经过rpm_to_rads转化后达到类似RPM的效果，实际单位并没有转换为rad/s
//-------------------------------------------------------------------------------------------------------------------
void Handle_encoder(Handle_encoder_zero *state,uint8_t high_byte_angle,uint8_t low_byt_angle,uint8_t high_byte_speed,uint8_t low_byt_speed)
{
    static const int MAX_encoder =  32768;   // 电机旋转一圈编码器反馈的绝对值角度
    static const int Half_MAX_encoder =   MAX_encoder/2;   // 最大编码器值的一半
    static int16_t actual_s = 0;   // 接收编码器的值并且将无符号的数据转化为有符号整形

    // 由于转速的分辨率为0.1，但是我没有除以10，所以经过rpm_to_rads转化后达到类似RPM的效果，实际单位并没有转换为rad/s
    float Current_speed = P1010B_Merge_uint8(high_byte_speed,low_byt_speed);
    state ->Actual_speed = rpm_to_rads(Current_speed);    // 获取当前速度(rpm转化为rad/s)

    uint16_t LOCATION = P1010B_Merge_uint8(high_byte_angle,low_byt_angle);     // 反馈绝对值角度，对于绝对值函数必须要使用无符号的，否则会出现错误
    actual_s = (int16_t)LOCATION;                       // 获取当前角度
    state->Actual_angle = (int)actual_s;

    // 首次读取初始化      必须先调用电流发送反馈的数据来获得初始转子位置，然后得到当前的角度，然后才可以开始中断计算
    if (state->first_reading) {
        state->last_angle = actual_s;
        state->first_reading = false;
        return;
    }

    int16_t delta = (int16_t)(state->Actual_angle - state->last_angle) ;

    if (state ->Actual_speed > 0.001) { // 顺时针
        if (delta < -16384) {
            state->raw_delta  = delta + MAX_encoder;
            state->over_zero_count++;
        } else {
            state->raw_delta  = delta;
        }
    } else if (state ->Actual_speed < -0.001) { // 逆时针
        if (delta > 16384) {
            state->raw_delta  = delta - MAX_encoder;
            state->over_zero_count--;
        } else {
            state->raw_delta  = delta;
        }
    } else { // 速度接近零
        state->raw_delta  = ((delta + 16384) % MAX_encoder) - 16384;
    }

    state->total_angle += state->raw_delta ;
    state->last_angle = state->Actual_angle ;
}



//-------------------------------------------------------------------------------------------------------------------
// 函数简介     查表法初始化COS   TODO
// 参数说明     CosLookupTable* table        引入结构体参数
// 返回参数     size                         用户规定的表最大数量，这里规定1000，为了节省内存你可以减小
// 使用示例
// 备注信息     基于弧度制
//-------------------------------------------------------------------------------------------------------------------
// 初始化查表结构体
void init_cos_lookup(CosLookupTable* table, int size) {
    if (!table || size <= 0 || size > 360) {
        fprintf(stderr, "Invalid table size.\n");
        return;
    }

    // 设置表的大小和步长
    table->table_size = size;
    table->step = (2 * M_PI) / size;  // 将一个周期 [0, 2π] 分成 size 份

    // 填充余弦表
    for (int i = 0; i < size; i++) {
        float angle = i * table->step;  // 当前角度
        table->cos_table[i] = cos(angle);  // 计算余弦值
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     查表法计算cos      TODO
// 参数说明     CosLookupTable* table       引入结构体参数
// 参数说明     angle                       输入的角度
// 使用示例
// 备注信息     基于弧度制
//-------------------------------------------------------------------------------------------------------------------
// 使用查表法计算余弦值
float lookup_cos(const CosLookupTable* table, float angle) {
    if (!table || table->table_size <= 0) {
        fprintf(stderr, "Invalid lookup table.\n");
        return 0.0;
    }

    // 将输入角度归一化到 [0, 2π)
    angle = fmod(angle, 2 * M_PI);
    if (angle < 0) {
        angle += 2 * M_PI;
    }

    // 计算索引位置
    int index = (int)(angle / table->step);

    // 确保索引在有效范围内
    if (index >= table->table_size) {
        index = table->table_size - 1;
    }
    return table->cos_table[index];
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     查表法初始化SIN   TODO
// 参数说明     CosLookupTable* table        引入结构体参数
// 返回参数     size                         用户规定的表最大数量，这里规定1000，为了节省内存你可以减小
// 使用示例
// 备注信息     基于弧度制
//-------------------------------------------------------------------------------------------------------------------
// 初始化查表结构体
void init_sin_lookup(SinLookupTable* table, int size) {
    if (!table || size <= 0 || size > 360) {
        fprintf(stderr, "Invalid table size.\n");
        return;
    }

    // 设置表的大小和步长
    table->table_size = size;
    table->step = (2 * M_PI) / size;  // 将一个周期 [0, 2π] 分成 size 份

    // 填充正弦表
    for (int i = 0; i < size; i++) {
        float angle = i * table->step;  // 当前角度
        table->sin_table[i] = sin(angle);  // 计算余弦值
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     查表法计算SIN      TODO
// 参数说明     CosLookupTable* table       引入结构体参数
// 参数说明     angle                       输入的角度
// 使用示例
// 备注信息     基于弧度制
//-------------------------------------------------------------------------------------------------------------------
// 使用查表法计算正弦值
float lookup_sin(const SinLookupTable* table, float angle) {
    if (!table || table->table_size <= 0) {
        fprintf(stderr, "Invalid lookup table.\n");
        return 0.0;
    }

    // 将输入角度归一化到 [0, 2π)
    angle = fmod(angle, 2 * M_PI);
    if (angle < 0) {
        angle += 2 * M_PI;
    }

    // 计算索引位置
    int index = (int)(angle / table->step);

    // 确保索引在有效范围内
    if (index >= table->table_size) {
        index = table->table_size - 1;
    }
    return table->sin_table[index];
}


//-------------------------------------------------------------------------------------------------------------------
// 函数简介 初始化ATAN2查表结构 TODO
// 参数说明 Atan2LookupTable* table 目标结构体
// 参数说明 table_size 表的大小(建议32-128)
// 使用示例 Atan2LookupTable atan_table; init_atan2_lookup(&atan_table, 64);
// 备注信息 表越大精度越高，但占用内存更多
//-------------------------------------------------------------------------------------------------------------------
// 初始化查表结构体
void init_atan2_lookup(Atan2LookupTable* table, int table_size) {
    if (!table || table_size <= 0) {
        fprintf(stderr, "Invalid parameters for atan2 table initialization.\n");
        return;
    }

// 限制表大小范围
    table->table_size = (table_size > MAX_ATAN2_TABLE_SIZE) ? MAX_ATAN2_TABLE_SIZE :
                        (table_size < MIN_ATAN2_TABLE_SIZE) ? MIN_ATAN2_TABLE_SIZE : table_size;

// 计算步长(比值范围[0,1])
    table->step = 1.0f / (table->table_size - 1);

// 预先计算atan值
    for (int i = 0; i < table->table_size; i++) {
        float ratio = i * table->step;
        table->atan_table[i] = (float)atan(ratio);
    }
}


//-------------------------------------------------------------------------------------------------------------------
// 函数简介 查表法计算ATAN2 TODO
// 参数说明 Atan2LookupTable* table 引入结构体参数
// 参数说明 y Y坐标值
// 参数说明 x X坐标值
// 使用示例 float angle = lookup_atan2(&atan_table, y, x);
// 备注信息 基于弧度制，返回范围[-π, π]，能正确处理过零问题
//-------------------------------------------------------------------------------------------------------------------
// 使用查表法计算atan2值
float lookup_atan2(const Atan2LookupTable* table, float y, float x) {
    if (!table || table->table_size <= 0) {
        fprintf(stderr, "Invalid atan2 lookup table.\n");
        return 0.0f;
    }

// 处理特殊情况
    if (x == 0.0f) {
        if (y == 0.0f) return 0.0f; // 原点，定义为0
        return (y > 0.0f) ? (float)(M_PI_2) : (float)(-M_PI_2); // y轴正负方向
    }

// 确定象限并转换到第一象限
    int quadrant = 0;
    float abs_x = (x >= 0.0f) ? x : -x;
    float abs_y = (y >= 0.0f) ? y : -y;
    float ratio;

// 利用对称性将问题限制在[0, 1]范围内
    if (abs_y <= abs_x) {
        ratio = abs_y / abs_x; // 在[0, 1]范围内
    } else {
        ratio = abs_x / abs_y; // 交换x,y，结果需要调整
        quadrant |= 4; // 标记需要π/2 - result
    }

// 确定象限
    if (x < 0.0f) quadrant |= 1; // 左半平面
    if (y < 0.0f) quadrant |= 2; // 下半平面

// 计算索引位置
    float index_val = ratio / table->step;
    int index = (int)index_val;

// 确保索引在有效范围内
    if (index >= table->table_size - 1) {
        index = table->table_size - 2; // 为插值保留空间
    }

// 线性插值提高精度 <button class="citation-flag" data-index="5">
    float fraction = index_val - (float)index;
    float result = table->atan_table[index] +
                   fraction * (table->atan_table[index + 1] - table->atan_table[index]);

// 根据是否交换了x,y进行调整
    if (quadrant & 4) {
        result = (float)M_PI_2 - result;
    }

// 根据象限调整最终结果
    switch (quadrant & 3) { // 只看低2位
        case 0: // 第一象限 (x>0, y>0)
            break;
        case 1: // 第二象限 (x<0, y>0)
            result = (float)M_PI - result;
            break;
        case 2: // 第四象限 (x>0, y<0)
            result = -result;
            break;
        case 3: // 第三象限 (x<0, y<0)
            result = -(float)M_PI - result;
            break;
    }

    return result;
}















